#include "Main.h"

#include <iostream>
#include <cstdlib>
#include <fstream>
#include <string>
#include <cmath>

#include <GL/glew.h>
#include <GL/glut.h>

#include "Body.h"
#include "Cube.h"
#include "Timer.h"
#include "Grid.h"

int main(int argc, char **argv) {
	
	totalNumberOfBodies = 30;
	if ( argc > 1 ) {
		totalNumberOfBodies = atoi( argv[1] );
	}
	std::cout << "Total number of bodies: " << totalNumberOfBodies << std::endl;

	initSimulation();

	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
	glutInitWindowSize(windowWidth, windowHeight);
	glutCreateWindow("Rigid body");
	glutDisplayFunc(display);
	glutMouseFunc(mouseFunc);
	glutMotionFunc(motionFunc);
	glutKeyboardFunc(keyboardFunc);
	glutReshapeFunc(reshapeFunc);
	initGL();

	//initCL();

	glutMainLoop();
	return 0;
}

void initSimulation() {
	createGrid();

	//Seed the random number generator used to generate colors for the bodies
	srand(time(NULL));

	totalNumberOfParticles = 0;
	vertexCount = 0;

	bodies = new Body*[totalNumberOfBodies];

	//Create cubes
	for (int i=0; i<totalNumberOfBodies; i++) {
		bodies[i] = new Cube();
		totalNumberOfParticles += 27;
		vertexCount += 24;
	}

	particles = new Particle*[totalNumberOfParticles];
	for (int i=0; i<totalNumberOfBodies; i++) {
		bodies[i]->populateParticleArray();
	}
	std::cout << "Number of Particles: " << totalNumberOfParticles << std::endl;
	std::cout << "VBO vertex count: " << vertexCount << std::endl;

	timer = new Timer();

}

void initGL() {

	GLfloat ambientLight[] = {0.2f, 0.2f, 0.2f, 1.0f};
	GLfloat diffuseLight[] = {0.7f, 0.7f, 0.7f, 1.0f};
	GLfloat specularLight[] = {1.0f, 1.0f, 1.0f, 1.0f};
	glLightfv(GL_LIGHT0, GL_AMBIENT, ambientLight);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuseLight);
	glLightfv(GL_LIGHT0, GL_SPECULAR, specularLight);

	float lightPos[4] = {0.0f, worldSize, 0.0f, 1.0f};
	glLightfv(GL_LIGHT0, GL_POSITION, lightPos);

	glEnable(GL_LIGHT0);
	glEnable(GL_LIGHTING);

	glEnable(GL_COLOR_MATERIAL);
	glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);

	glEnable(GL_DEPTH_TEST);

	glMatrixMode(GL_PROJECTION);
	gluPerspective(60.0, (GLfloat) windowWidth / (GLfloat) windowHeight, 0.1, 100.0);

	glPointSize(3.0f);

	vertexData = new float[vertexCount*3*3];
	normalData = &(vertexData[vertexCount*3]);
	colorData = &(vertexData[vertexCount*3*2]);
	/*
	normalData = new float[vertexCount*3];
	colorData = new float[vertexCount*3];
	*/

	for (int i=0; i<vertexCount*3; i++) {
		vertexData[i] = 0.0f;
		normalData[i] = 0.0f;
		colorData[i] = 0.0f;
	}

	for (int i=0; i<totalNumberOfBodies; i++) {
		int vertexIndex = i*24*3;
		bodies[i]->updateColorArray(vertexIndex);
	}

	particleVertexData = new float[totalNumberOfParticles*3*2];
	particleColorData = &(particleVertexData[totalNumberOfParticles*3]);
	/*
	particleColorData = new float[totalNumberOfParticles*3];
	*/

	for (int i=0; i<totalNumberOfParticles*3; i++) {
		particleVertexData[i] = 0.0f;
		particleColorData[i] = 0.0f;
	}

	for (int i=0; i<totalNumberOfBodies; i++) {
		int vertexIndex = i*27*3;
		bodies[i]->updateParticleColorArray(vertexIndex);
	}

	glewInit();
	glGenBuffers(1, &vbo);
}

void display() {
	timer->beginFrame();

	stepSimulation(timeDelta);
	
	glutPostRedisplay();

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(0.0, translateY, translateZ);
	glRotatef(rotateX, 1.0, 0.0, 0.0);
	glRotatef(rotateY, 0.0, 1.0, 0.0);
	drawGroundPlane();
	
	if (onlyDrawParticles) {
		//Update cube particle VBO arrays
		for (int i=0; i<totalNumberOfBodies; i++) {
			int vertexIndex = i*27*3;
			bodies[i]->updateParticleVBO(vertexIndex);
		}
		drawParticles();

	} else {
		//Update cube VBO arrays
		for (int i=0; i<totalNumberOfBodies; i++) {
			int vertexIndex = i*24*3;
			bodies[i]->updateVBO(vertexIndex);
		}
		drawBodies();
	}

	glutSwapBuffers();

	timer->endFrame();

	if (timer->isTimeToUpdate()) {
		float fps = timer->getFPS();
		char title[64];
		sprintf(title, "Rigid Body | %d fps", (int)fps);
		glutSetWindowTitle(title);
	}
}

void drawParticles() {
	int bufferSize = totalNumberOfParticles*3*sizeof(float);
	glBindBuffer(GL_ARRAY_BUFFER, vbo);
	glBufferData(GL_ARRAY_BUFFER, 2*bufferSize, particleVertexData, GL_DYNAMIC_DRAW);
	//glBufferSubData(GL_ARRAY_BUFFER, bufferSize, bufferSize, particleColorData);

	glVertexPointer(3, GL_FLOAT, 0, NULL);
	glColorPointer(3, GL_FLOAT, 0, (void*)bufferSize);

	glEnableClientState(GL_VERTEX_ARRAY);
	glEnableClientState(GL_COLOR_ARRAY);

	glDrawArrays(GL_POINTS, 0, totalNumberOfParticles);

	glDisableClientState(GL_VERTEX_ARRAY);
	glDisableClientState(GL_COLOR_ARRAY);
}

void drawBodies() {
	int bufferSize = vertexCount*3*sizeof(float);

	glBindBuffer(GL_ARRAY_BUFFER, vbo);
	glBufferData(GL_ARRAY_BUFFER, 3*bufferSize, vertexData, GL_DYNAMIC_DRAW);
	//glBufferSubData(GL_ARRAY_BUFFER, bufferSize, bufferSize, normalData);
	//glBufferSubData(GL_ARRAY_BUFFER, 2*bufferSize, bufferSize, colorData);

	glVertexPointer(3, GL_FLOAT, 0, NULL);
	glNormalPointer(GL_FLOAT, 0, (void*)bufferSize);
	glColorPointer(3, GL_FLOAT, 0, (void*)(2*bufferSize));

	glEnableClientState(GL_VERTEX_ARRAY);
	glEnableClientState(GL_NORMAL_ARRAY);
	glEnableClientState(GL_COLOR_ARRAY);

	glDrawArrays(GL_QUADS, 0, vertexCount); 

	glDisableClientState(GL_VERTEX_ARRAY);
	glDisableClientState(GL_NORMAL_ARRAY);
	glDisableClientState(GL_COLOR_ARRAY);

}


void mouseFunc(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		if (button == GLUT_LEFT_BUTTON) {
			leftButtonPressed = true;
		} else if (button == GLUT_RIGHT_BUTTON) {
			rightButtonPressed = true;
		}
	} else if (state == GLUT_UP) {
		leftButtonPressed = false;
		rightButtonPressed = false;
	}
	oldMouseX = x;
	oldMouseY = y;
	glutPostRedisplay();
}

void motionFunc(int x, int y) {
	float dx, dy;
	dx = x - oldMouseX;
	dy = y - oldMouseY;

	if (leftButtonPressed) {
		rotateX += dy * 0.2;
		rotateY += dx * 0.2;
	} else if (rightButtonPressed) {
		translateZ += dy * 0.01;
	}

	oldMouseX = x;
	oldMouseY = y;
	glutPostRedisplay();
}

void resetSimulation1() {
	for (int i=0; i<totalNumberOfBodies; i++) {

		int count = i+1;
		float side = particleRadius * 3.0f;

		float x,y,z;
		x = (count % 2) * 1.9f * side;
		y = count * 3.0f * side;
		z = ((count % 4)/2) * 1.9f * side;

		float newPosition[3] = {x, y, z};
		bodies[i]->reset(newPosition);
	}
}

void resetSimulation2() {
	int i = 0;
	int y=0;
	int towerSize = 5;
	while (i < totalNumberOfBodies) {
		for (int x=-towerSize; x<=towerSize; x++) {
			for (int z=-towerSize; z<=towerSize; z++) {
				if (i < totalNumberOfBodies) {
					float position[3];
					position[0] = x*6.0f*particleRadius;
					position[1] = y*6.0f*particleRadius + 3.0f*particleRadius;
					position[2] = z*6.0f*particleRadius;
					bodies[i]->reset(position);
					i++;
				}
			}
		}
		y++;
	}
}

void keyboardFunc(unsigned char key, int x, int y) {
	switch (key) {
	case 'p':
	case 'P':
		onlyDrawParticles = !onlyDrawParticles;
		break;
	case '1':
		resetSimulation1();
		break;
	case '2':
		resetSimulation2();
		break;
	case 'q':
	case 'Q':
		std::exit(0);
		break;
	}
	glutPostRedisplay();	
}

void stepSimulation(float delta) {

	for (int i=0; i<totalNumberOfBodies; i++) {
		bodies[i]->updateParticleValues();
	}

	updateGrid();

	for (int i=0; i<totalNumberOfBodies; i++) {
		bodies[i]->updateMomenta(delta);	
	}

	for (int i=0; i<totalNumberOfBodies; i++) {
		bodies[i]->performStep(delta);
	}
}

void drawGroundPlane() {
	glColor3f(0.3f, 0.3f, 0.3f);
	glBegin(GL_QUADS);
	glNormal3fv(&groundNormal[0]);
	glVertex3fv(&ground[0][0]);
	glVertex3fv(&ground[1][0]);
	glVertex3fv(&ground[2][0]);
	glVertex3fv(&ground[3][0]);
	glEnd();
}

void reshapeFunc(int w, int h) {
	windowWidth = w;
	windowHeight = h;

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) windowWidth / (GLfloat) windowHeight, 0.1, 100.0);
	glViewport(0,0,w,h);
}


